#include "geometry.hpp"

using namespace std;

/*http://www.ai2news.com/blog/2950588/
直线方程为ax+b=y,构建最小二乘方程，分别对参数a和b求偏导，令偏导为0，则直接可求得a和b;这种方式使得离群点也参与
直线拟合运算，会导致拟合结果出现较大误差;
*/
bool FitLine(vector<Eigen::Vector2d> &points, float &a, float &b)
{
    float sum_y = 0.0;
    float sum_x = 0.0;
    float sum_x2 = 0.0;
    float sum_xy = 0.0;

    int num = int(points.size());
    for (int i = 0; i < num; ++i)
    {
        auto x = points[i](0);
        auto y = points[i](1);
        sum_x += x;
        sum_y += y;
        sum_x2 += x * x;
        sum_xy += x * y;
    }
    float tmp = num * sum_x2 - sum_x * sum_x;
    if (abs(tmp) > 0.000001f)
    {
        a = (num * sum_xy - sum_x * sum_y) / tmp;
        b = (sum_x2 * sum_y - sum_x * sum_xy) / tmp;
        return true;
    }
    return false;
}

void RansacFitLine(vector<Eigen::Vector2d> &points, int iter_num, float alpha, float &a, float &b)
{
    const ransac_num = 2;
    // 记录随机索引
    int random_idx[2];
    srand((unsigned)time(NULL));
    // 最大内点集合size
    int max_inliers_num = 0;
    int point_size = points.size();
    // 临时存储每次迭代的内点
    vector<Eigen::Vector2d> inliers;
    // 最大内点集合
    vector<Eigen::Vector2d> max_inliers;
    for (int i = 0; i < iter_num; i++)
    {
        GenerateRandomNumber(point_size, ransac_num, random_idx);
        // 随机选择2个点
        vector<Eigen::Vector2d> picked;
        for (int j = 0; j < ransac_num; j++)
        {
            int index = random_idx[j];
            picked.push_back(points[index]);
        }
        // 使用随机选择的两点计算直线
        float aa = 0, bb = 0;
        FitLine(picked, ransac_num, aa, bb);
        // 计算所有点到该直线的距离，并记录最大最小距离
        float dist_min = 99999999.9f;
        float dist_max = -99999999.9f;
        vector<float> dist_list;
        for (int j = 0; j < point_size; j++)
        {
            const auto &point = points[j];
            float distance = abs(aa * point(0) - point(1) + bb) / sqrtf(aa * aa + 1.0f);
            dist_list.push_back(distance);
            dist_min = MIN(dist_min, distance);
            dist_max = MAX(dist_max, distance);
        }
        // 根据0~1的α值和最大最小距离计算阈值
        float threld = dist_min + (dist_max - dist_min) * alpha;
        vector<Eigen::Vector2d>().swap(inliers);
        for (int j = 0; j < point_size; j++)
        {
            // 判断如果点距离小于阈值则将该点加入内点集合
            if (dist_list[j] < threld)
                inliers.push_back(points[j]);
        }
        // 记录历史最大内点集合
        if (max_inliers_num < inliers.size())
        {
            max_inliers_num = inliers.size();
            max_inliers.swap(inliers);
        }
    }
    // 判断如果历史最大内点数大于等于2，则使用历史最大内点数集合来计算直线
    if (max_inliers_num >= ransac_num)
    {
        lineplofit(max_inliers, max_inliers.size(), a, b);
        return true;
    }
    return false;
}

int main(int argc, char *argv[])
{
    return 1;
}
